extends Node
class_name  MovePlan
# 计算飞剑运动计划（一维）
# 输入：当前速度，距离，Fmax,m
# 使用：制定计划后，第一程加速动，第二程减速运动，每次调用新当前时间。二维运动应使用两个计划(plan_x,plan_y)
var target=-1#目标点位置，只起一个标志作用，用于判断是否重做计划
var t_now =0#当前时间
var f_fore =0#第一程用的力, 向前
var t_turn =0#转向时间
var f_back =0#第二程用的力，向后
var t_stop =0#停止时间

#制定计划
#输入：目标点位置，当前速度，目标点距离，最大力，质量
func make_plan(target,v0,d,Fmax,m):
	if abs(d) < 100:
		Fmax *= abs(d)/100
	self.target=target
	var a = Fmax/m  # 动力可产生的最大加速度（无方向）
	var a_ = -v0*v0/(2*d) # 根据刹车距离计算出的加速度（方向与目标距离相反）
	if d*v0>0 and a<abs(a_):#需要急刹车
		t_now = 0
		f_fore = 0
		t_turn =0
		f_back = m*a_
		t_stop = - v0/a_
	else:#不需要急刹车
		t_now =0
		f_fore = sign(d)*Fmax
		a=sign(d)*a#a与d同号
		var sqrt_delta = sign(d)*sqrt(2*v0*v0+4*a*d)
		t_turn =(-2*v0+sqrt_delta)/(2*a)
		f_back = -f_fore
		t_stop = (2*a*t_turn+v0)/a
	

func target_changed(t):
	return target!=t
	
func read_and_update_plan(delta):
	if t_now<t_turn:
		t_now+=delta
		return f_fore
	elif t_now<t_stop:
		t_now+=delta
		return f_back
	else:
		return 0
